#include "stm32f4xx.h"                  // Device header
#include "usart.h" 
#include "delay.h" 
#include "OLED.h"
#include "MPU6050.h"
#include "kalman.h"
#include "Key.h"
#include "led.h"

uint8_t ID;								
short AX[3], GX[3];	
uint8_t KeyNum;
float X[500],Y[500],XE,YE;
int i=0;
int count=0;   //1ī݆˽

unsigned int mod2_flag=0;   //ģʽ2èݤЪģʽéҪ־λ
u8 p_flag=0;  //ʨ׃ָ֫
u8 mod=0;   //0:ژҕģʽ   1:lѸ٤طģʽ   2:ݤЪ1ī٤طģʽ  
int f_num=1;     //öԥƵÊ1Hz--200Hz
int d_num=50;    //öԥ࠭׈50--1000us
int i_num=90;    //֧·ǿ׈0--100mA

u8 lcd1[20]={0};
u8 lcd2[20]={0};	
u8 lcd3[20]={0};
u8 lcd4[20]={0};
int main(void)
{
	
	OLED_Init();		
    I2cMaster_Init();
	MPU6050_Init();
    KEY_Init();
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//ʨ׃ϵͳא׏ԅЈܶؖة2
	delay_init(168);  //Եʼۯғʱگ˽
	LED_Init();				//ԵʼۯLED׋ࠚ
	LCD_TM9665_Init();                                        //ДʾǁԵʼۯ
	LCD_TM9665_Print_Str_Ascii8x16("start",0,0);

 	TIM3_Int_Init(10000-1,8400-1);	//֨ʱǷʱד84MìؖƵϵ˽8400ì̹Ӕ84M/8400=10Khzք݆˽ƵÊì݆˽5000ՎΪ500ms     


	CN=0;
	CP=1;   //ԵʼΪغѹ50V
	
	while(1)
	{
    GPIO_SetBits(GPIOD, GPIO_Pin_2);		
    if(count>0)
		{		
      count=0;

		
			if(mod==0)
			{	
	      TIM_Cmd(TIM3, DISABLE);  //ʹŜTIMx		
			}
			else if(mod==1)
			{
			  TIM_Cmd(TIM3, ENABLE);  //ʹŜTIMx		
			}
			else if(mod==2)
			{
			  if(mod2_flag==0)
				{
					mod2_flag=1;
					TIM_Cmd(TIM3, ENABLE);  //ʹŜTIMx	
				}
				else
				{
					mod2_flag=0;
					TIM_Cmd(TIM3, DISABLE);  //ʹŜTIMx	
				}		
			}
			
			lcd1[0]='M';
			lcd1[1]=':';
			lcd1[2]=mod+'0';
			lcd1[3]=' ';
			lcd1[4]='P';
			lcd1[5]=':';
			lcd1[6]=p_flag+'0';				
			LCD_TM9665_Print_Str_Ascii8x16(lcd1,0,0);
			
			lcd2[0]='F';
			lcd2[1]=':';
			lcd2[2]=f_num/100%10+'0';
			lcd2[3]=f_num/10%10+'0';
			lcd2[4]=f_num/1%10+'0';
			lcd2[5]='H';
			lcd2[6]='z';
			LCD_TM9665_Print_Str_Ascii8x16(lcd2,2,0);
			
			lcd3[0]='D';
			lcd3[1]=':';
			lcd3[2]=d_num/100%10+'0';
			lcd3[3]=d_num/10%10+'0';
			lcd3[4]=d_num/1%10+'0';
			lcd3[5]='m';
			lcd3[6]='s';
			LCD_TM9665_Print_Str_Ascii8x16(lcd3,4,0);
				
			lcd4[0]='I';
			lcd4[1]=':';
			lcd4[2]=i_num/100%10+'0';
			lcd4[3]=i_num/10%10+'0';
			lcd4[4]=i_num/1%10+'0';
			lcd4[5]='m';
			lcd4[6]='A';
			LCD_TM9665_Print_Str_Ascii8x16(lcd4,6,0);
		}
	
		if(key_sel==0)   //дݼָ֫ʨ׃
		{
		  if(p_flag<3)
			{
			  p_flag++;
			}
      else
			{
			  p_flag=0;
			}
      while(key_sel==0);			
		}			
		
		if(p_flag==0)   //ģʽʨ׃
		{
		  if(key_add==0)
			{
			  if(mod<2)
				{
				  mod++;
				}
				while(key_add==0);  //ֈսдݼˍ؅
			}
			
			if(key_sub==0)
			{
			  if(mod>0)
				{
				  mod--;
				}
				while(key_sub==0);  //ֈսдݼˍ؅
			}
		}
		
		if(p_flag==1)   //ƵÊʨ׃
		{
		  if(key_add==0)
			{
			  if(f_num<200)
				{
				  f_num++;
				}
				while(key_add==0);  //ֈսдݼˍ؅				
				TIM3_Int_Init(10000/f_num-1,7199);          //10Khzք݆˽ƵÊì݆˽ս5000Ϊ500ms
			}
			
			if(key_sub==0)
			{
			  if(f_num>0)
				{
				  f_num--;
				}
				while(key_sub==0);  //ֈսдݼˍ؅
				TIM3_Int_Init(10000/f_num-1,7199);          //10Khzք݆˽ƵÊì݆˽ս5000Ϊ500ms
			}
		}
		
		if(p_flag==2)   //öԥ࠭׈ʨ׃
		{
		  if(key_add==0)
			{
			  if(d_num<999)
				{
				  d_num++;
				}
				while(key_add==0);  //ֈսдݼˍ؅
			}
			
			if(key_sub==0)
			{
			  if(d_num>0)
				{
				  d_num--;
				}
				while(key_sub==0);  //ֈսдݼˍ؅
			}
		}
		
		if(p_flag==3)   //֧·ʨ׃
		{
		  if(key_add==0)
			{
			  if(i_num<100)
				{
				  i_num++;
				}
				while(key_add==0);  //ֈսдݼˍ؅
			}
			
			if(key_sub==0)
			{
			  if(i_num>0)
				{
				  i_num--;
				}
				while(key_sub==0);  //ֈսдݼˍ؅
			}
		}
		
		count++;
		delay_ms(10);    //10ہī
	
	
		KeyNum = Key_GetNum();		//获取按键键码
		if (KeyNum == 1){
		XE=0;
        YE=0;			
	for(i=0;i<50;i++){	
		KeyNum = Key_GetNum();
		Angle_Calcu();
		OLED_DispStr(1,1,"X:",&tFont12);
		X[i]=Angle_X_Final;
		if(X[i]>=10){
			GPIO_ResetBits(GPIOD, GPIO_Pin_2);		//灏哖B12寮曡剼璁剧疆涓轰綆鐢靛钩锛岃渹楦ｅ櫒楦ｅ彨
		delay_ms(100);							//寤舵椂100ms
		GPIO_SetBits(GPIOD, GPIO_Pin_2);}
		OLED_DisDigital(15, 1, X[i] , &tFont12);
	    XE=XE+X[i]/500;
		OLED_DispStr(1,15,"Y:",&tFont12);
		Y[i]=Angle_Y_Final;
		OLED_DisDigital(15,15,Y[i] , &tFont12);
		OLED_ClrScr(0x00);
		YE=YE+Y[i]/500;
		if(KeyNum==2){
	     break;
	   }	
		OLED_ClrScr(0x00);
		}
       
}
        OLED_DispStr(1,30,"XE:",&tFont12);
        OLED_DisDigital(20,30, XE, &tFont12);
        OLED_DispStr(1,45,"YE:",&tFont12);
        OLED_DisDigital(20,45, YE, &tFont12);
        OLED_ClrScr(0x00);

}
}